/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    GaitScheduler.cpp
  * @author  Silang WU 1134138135@qq.com
  * @brief   Schedule the gait pattern for 4 legs independently in Body frame.
  * @date    2020-08-20
  * 
================================================================================
                            How to use this Module
================================================================================
  * @note    
  *         The following gaits supported for now:
  *         - Stand Up/Down, Walk, Trot, Pace, Gallop,Jump.
  *         The phase sequence:RF(right forward leg)->LF(left forward leg)
  *                            LB(left backward leg)->RB(right backward leg)                         
  *         \li Stand:  phase     0.0->0.5->0.0->0.5
  *                     period    0.6                     Unit:second
  *         \li Walk:   phase     0.0->0.5->0.25->0.75
  *                     period    0.6                     Unit:second
  *         \li Trot:   phase     0.0->0.5->0.25->0.75
  *                     period    0.6                     Unit:second
  *         \li Pace:   phase     0.0->0.5->0.5->0.0
  *                     period    0.36                    Unit:second
  *         \li Gallop: phase     0.0->0.5->0.5->0.0
  *                     period    0.36                    Unit:second
  *         \li Jump:   phase     0.0->0.5->0.5->0.0
  *                     period    It must be calculated or tested.
  *         Here is the example:
  *             |1 GaitScheduler<double> gaittest;
  *             |2 gaittest.initialize();
  *             |3 gaittest.gaitGenerate();
  *         
  *             gaittest.showInfo() is called in gaitGenerate().
  *             The default gait is STANCE.
  *             If you want to change teh default gait before generating gait,
  *             use gaittest.changeGait() before gaittest.gaitGenerate().
  *         
  *         The following trajectory types supported for now:
  *         - Sinusoidal curve.
  *         - Polynomial curve
  * 
  *         To get more information for the trajectory,please contact Silang Wu(1134138135@qq.com).
  * @warning 
  *     - At least C++11 is required.														 
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "Motion_Scheduler/GaitScheduler.h"
#include <iostream>

/* Private defines -----------------------------------------------------------*/
using namespace Quadruped;
/* Founctions ----------------------------------------------------------------*/
/**
 * @brief  Initialize nearly all the gait data.
 * @retval None.
 */

/*========================= Gait Part of Gait Scheduler ============================*/

/**
 * @brief  Initialize the "Gait-Scheduler".
 * @note   This function selects STAND_UP as default gait.
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::initialize()
{
    gaitData.current_gait = GaitType::STAND_UP;
    /*In this step,we set next_gait as current_gait.*/
    gaitData.zero();
    TrajParamSetZero();
    /*Set trajectory parameters.*/
    Traj_Param_Init(0.2, Sinusoidal_Trajectory, cartesianSpace);

    /*sample every 0.02/1.00 (phase offset) */
    SetPhaseInterval(0.02);
    createGait(gaitData.next_gait);
}

/** 
 * @brief  Set gait parameters for the selected gait.
 * @note   RF, LF, LB and RB which are defined in "dynamicModel.h" ,respectively refer to 0,1,2 and 3.
 * @param  gait_type:specifies the gait type,which can be chosen from the following options:
 *                   STAND_UP,STAND_DOWN,WALK,TROT,PACE,GALLOP.
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::createGait(GaitType gait_type)
{
    gaitData.next_gait = gait_type;
    switch (gaitData.next_gait)
    {
    //Stand-up:step on the ground in the trot type with body at a high position.
    case GaitType::STAND_UP:
        gaitData._periodTime = 1.5;
        gaitData._phaseOffset << 0.0, 0.0,0.0,0.0;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion << 0.0,0.0,0.0,0.0; //the proportion of swing time in the whole period time.
        break;
    //Stand-up:step on the ground in the trot type with body at a low position.
    case GaitType::STAND_DOWN:
        gaitData._periodTime = 0.4;
        gaitData._phaseOffset << 0.0,0.0,0.0,0.0;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion << 0.0,0.0,0.0,0.0; //the proportion of swing time in the whole period time.
        break;
    case GaitType::WALK:
        gaitData._periodTime = 1.24;
        gaitData._phaseOffset << 0.0, 0.5, 0.25, 0.75;       //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion << 0.25, 0.25, 0.25, 0.25; //the proportion of swing time in the whole period time.
        break;
    case GaitType::TROT:
        gaitData._periodTime = 0.28;
        gaitData._phaseOffset << 0.0, 0.5, 0.0, 0.5;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion << 0.5, 0.5, 0.5, 0.5; //the proportion of swing time in the whole period time.
        break;
    case GaitType::CANTER:
        gaitData._periodTime = 0.4;
        gaitData._phaseOffset<<0.0,0.2,0.0,0.7;
        gaitData._swingProportion<< 0.6,0.6,0.6,0.6;
    case GaitType::PACE:
        gaitData._periodTime = 0.5;
        gaitData._phaseOffset << 0.0, 0.5, 0.5, 0.0;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion << 0.5, 0.5, 0.5, 0.5; //the proportion of swing time in the whole period time.
        break;
    case GaitType::GALLOP:
        gaitData._periodTime = 0.4;
        gaitData._phaseOffset<< 0.0, 0.9, 0.4, 0.5;
        gaitData._swingProportion<<0.7, 0.7, 0.7, 0.7; 
        //gaitData._phaseOffset << 0.0, 0.75, 0.25, 0.5;       //the phase offset of each leg relative to initial phase.
        //gaitData._swingProportion << 0.75, 0.75, 0.75, 0.75; //the proportion of swing time in the whole period time.
        break;
    case GaitType::JUMP:
        gaitData._periodTime = 1.5;
        gaitData._phaseOffset << 0.0, 0.0,0.0,0.0;
        //gaitData._swingProportion << 0.5, 0.5, 0.5, 0.5; //the proportion of swing time in the whole period time.
        break;
    case GaitType::BOUNCE:
        gaitData._periodTime = 2.0;
        gaitData._phaseOffset << 0.0, 0.0, 0.0, 0.0;     //the phase offset of each leg relative to initial phase.
        //gaitData._swingProportion << 0.5, 0.5, 0.5, 0.5; //the proportion of swing time in the whole period time.
        break;
    default:
        break;
    }
    gaitData._swingTime << gaitData._swingProportion(0) * gaitData._periodTime,
        gaitData._swingProportion(1) * gaitData._periodTime,
        gaitData._swingProportion(2) * gaitData._periodTime,
        gaitData._swingProportion(3) * gaitData._periodTime;
    gaitData._stanceProportion << (1 - gaitData._swingProportion(0)), (1 - gaitData._swingProportion(1)),
        (1 - gaitData._swingProportion(2)), (1 - gaitData._swingProportion(3));
    gaitData._stanceTime << gaitData._stanceProportion(0) * gaitData._periodTime,
        gaitData._stanceProportion(1) * gaitData._periodTime,
        gaitData._stanceProportion(2) * gaitData._periodTime,
        gaitData._stanceProportion(3) * gaitData._periodTime;
    gaitData.current_gait = gaitData.next_gait;
}

/**
 * @brief   Set the traj height, starting point & final point for the trajectory of specified foot
 * @param   startPoint:The start point in the foot trajectory.
 * @param   finalPoint:The final point in the foot trajectory.
 * @param   trajHeight:Specify the height of trajectory between start point & final point.
 * @param   foot_id:The foot that is chosen.
 */
template <typename T>
void GaitScheduler<T>::SetFootPlacement(const Vec3<T> &startPoint, const Vec3<T> &finalPoint,T trajHeight,int foot_id)
{
    if(gaitData.current_gait ==GaitType::STAND_UP && gaitData.next_gait == GaitType::STAND_UP)
    {
        if(gaitData._exeStanceEndFlag==1 && gaitData._pFinal[foot_id] != finalPoint)
        {   
            gaitData._exeStanceEndFlag =0;
            gaitData._pStart[foot_id]=gaitData._pFinal[foot_id];
            gaitData._pFinal[foot_id]=finalPoint;  
        }
        else if(gaitData._runTimes==0)
        {
            //====================set  startPos======================//
            if(gaitData._startSignal)
            {
                gaitData._pStart[foot_id]=startPoint;
                gaitData._startSignal--;
            }
            //===================================================//
                gaitData._pFinal[foot_id]=finalPoint;
                gaitData._trajHgt(foot_id)=trajHeight;
        }
    }
    else if(gaitData.current_gait ==GaitType::JUMP && gaitData.next_gait == GaitType::JUMP || (gaitData.current_gait ==GaitType::BOUNCE && gaitData.next_gait == GaitType::BOUNCE))
    {
        gaitData._pStart[foot_id]=startPoint;
        gaitData._pFinal[foot_id]=finalPoint;
    }
    else
    {
        if(gaitData._trajFootState[foot_id] == ContactState::stanceState)
        {
            gaitData._pStart[foot_id]=startPoint;
            gaitData._trajHgt(foot_id)=trajHeight;
        }
        gaitData._pFinal[foot_id]=finalPoint;
    }
}


template <typename T>
void GaitScheduler<T>::TransitionToWalk()
{
    static float tempTime=0.0;
    tempTime+=0.25;

    if(gaitData.current_gait==GaitType::STAND_UP)
    {
        gaitData._vBody=gaitData._v_CoM;
        createGait(gaitData.next_gait);
        gaitData.current_gait=gaitData.next_gait;
        gaitData._gaitChangeFlag=0;
    }
    else if (gaitData.current_gait==GaitType::TROT)
    {
        gaitData._periodTime = 0.5;
        gaitData._phaseOffset << 0.0,0.5,0.0*(1-tempTime)+0.25*tempTime,0.5*(1-tempTime)+0.75*tempTime;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion <<0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime; //the proportion of swing time in the whole period time.
        gaitData._stanceProportion <<   (1 - gaitData._swingProportion(0)), (1 - gaitData._swingProportion(1)),
                                        (1 - gaitData._swingProportion(2)), (1 - gaitData._swingProportion(3));
        gaitData._swingTime  << gaitData._swingProportion(0) * gaitData._periodTime,
                                gaitData._swingProportion(1) * gaitData._periodTime,
                                gaitData._swingProportion(2) * gaitData._periodTime,
                                gaitData._swingProportion(3) * gaitData._periodTime;
        gaitData._stanceTime << gaitData._stanceProportion(0) * gaitData._periodTime,
                                gaitData._stanceProportion(1) * gaitData._periodTime,
                                gaitData._stanceProportion(2) * gaitData._periodTime,
                                gaitData._stanceProportion(3) * gaitData._periodTime;
    }
    else if (gaitData.current_gait==GaitType::CANTER)
    {
        /* code */
    }
    else if (gaitData.current_gait==GaitType::GALLOP)
    {
        /* code */
    }
    if(tempTime==1.0)
    {
        tempTime=0.0;
        gaitData._gaitChangeFlag=0;//Gait Change Finished.
        gaitData.current_gait=gaitData.next_gait;
    }
}

/**
 * @brief  Gait transition to TROT.
 */
template <typename T>
void GaitScheduler<T>::TransitionToTrot()
{
    static float tempTime=0.0;
    tempTime+=0.10;
    if (gaitData.current_gait==GaitType::STAND_UP)
    {
        // createGait(GaitType::TROT);
        gaitData._vBody=gaitData._v_CoM*tempTime;
        // gaitData._gaitChangeFlag=0;
        // gaitData.current_gait=gaitData.next_gait;
        gaitData._periodTime=0.6;
        gaitData._phaseOffset << 0.0, 0.5, 0.0, 0.5;//the phase offset of each leg relative to initial phase.
        gaitData._swingProportion <<0.50*tempTime,
                                    0.50*tempTime,
                                    0.50*tempTime,
                                    0.50*tempTime; //the proportion of swing time in the whole period time.
    }
    else if (gaitData.current_gait==GaitType::WALK)
    {
        gaitData._periodTime = 1.24*(1-tempTime)+0.5*tempTime;
        gaitData._phaseOffset << 0.0,0.5,0.25*(1-tempTime)+0.0*tempTime,0.75*(1-tempTime)+0.5*tempTime;     //the phase offset of each leg relative to initial phase.
        gaitData._swingProportion <<0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime,
                                    0.5*(1-tempTime)+0.25*tempTime; //the proportion of swing time in the whole period time.
    }
        gaitData._stanceProportion <<   (1 - gaitData._swingProportion(0)), (1 - gaitData._swingProportion(1)),
                                        (1 - gaitData._swingProportion(2)), (1 - gaitData._swingProportion(3));
        gaitData._swingTime  << gaitData._swingProportion(0) * gaitData._periodTime,
                                gaitData._swingProportion(1) * gaitData._periodTime,
                                gaitData._swingProportion(2) * gaitData._periodTime,
                                gaitData._swingProportion(3) * gaitData._periodTime;
        gaitData._stanceTime << gaitData._stanceProportion(0) * gaitData._periodTime,
                                gaitData._stanceProportion(1) * gaitData._periodTime,
                                gaitData._stanceProportion(2) * gaitData._periodTime,
                                gaitData._stanceProportion(3) * gaitData._periodTime;
        if(tempTime>=1.0)
        {
            tempTime=0.0;
            gaitData._gaitChangeFlag=0;//Gait Change Finished.
            gaitData.current_gait=gaitData.next_gait;
        }
}

/**
 * @brief  Gait transition to CANTER.
 */
template <typename T>
void GaitScheduler<T>::TransitionToCanter()
{
    static float tempTime=0.0;
    tempTime+=0.10;
    if (gaitData.current_gait==GaitType::STAND_UP||gaitData.current_gait==GaitType::WALK)
    {
       tempTime=0.0;
        TransitionToTrot();
        if (gaitData._gaitChangeFlag==0)
        {
            gaitData.current_gait=GaitType::TROT;
            gaitData.next_gait=GaitType::CANTER;
            gaitData._exeTrotToCanterFlag=1;
        }
       gaitData._gaitChangeFlag=1;
    }
    else if(gaitData.current_gait==GaitType::TROT)
    {
        gaitData._vBody=gaitData._vBody+(Vec3<T>(0.02,0,0));
        gaitData._periodTime=0.2*tempTime+0.6*(1-tempTime);
        gaitData._phaseOffset<< 0.0,
                                0.25*tempTime+0.5*(1-tempTime),
                                0.0,
                                0.75*tempTime+0.5*(1-tempTime);
        gaitData._swingProportion<< 0.65*tempTime + 0.5*(1-tempTime),
                                    0.65*tempTime + 0.5*(1-tempTime),
                                    0.65*tempTime + 0.5*(1-tempTime),
                                    0.65*tempTime + 0.5*(1-tempTime);
    }
    gaitData._stanceProportion<<1-gaitData._swingProportion(0),
                                1-gaitData._swingProportion(1),
                                1-gaitData._swingProportion(2),
                                1-gaitData._swingProportion(3);
    gaitData._swingTime<<   gaitData._swingProportion(0) * gaitData._periodTime,
                            gaitData._swingProportion(1) * gaitData._periodTime,
                            gaitData._swingProportion(2) * gaitData._periodTime,
                            gaitData._swingProportion(3) * gaitData._periodTime;
    gaitData._stanceTime<<  gaitData._stanceProportion(0) * gaitData._periodTime,
                            gaitData._stanceProportion(1) * gaitData._periodTime,
                            gaitData._stanceProportion(2) * gaitData._periodTime,
                            gaitData._stanceProportion(3) * gaitData._periodTime;
    if (tempTime>=1)
    {
        tempTime=0.0;
        gaitData._gaitChangeFlag=0;//Gait Change Finished.
        gaitData.current_gait=gaitData.next_gait;
    }
}

/**
 * @brief  Gait transition to GALLOP.
 */
template <typename T>
void GaitScheduler<T>::TransitionToGallop()
{
    static float tempTime;
    tempTime+=0.25;
    if (gaitData.current_gait==GaitType::WALK||gaitData.current_gait==GaitType::STAND_UP)
    {
        tempTime=0.0;
        TransitionToTrot();
    }
    else if(gaitData.current_gait==GaitType::TROT)
    {
        tempTime=0.0;
        TransitionToCanter();
    }
    else if (gaitData.current_gait==GaitType::CANTER)
    {
        gaitData._periodTime=0.4;//When the period time of canter is equal to gallop's,this line can be deleted;
        gaitData._phaseOffset<< 0.0,
                                0.9*tempTime+0.2*(1-tempTime),
                                0.4*tempTime,
                                0.5*tempTime+0.7*(1-tempTime);
        gaitData._swingProportion<< 0.7*tempTime+0.6*(1-tempTime),
                                    0.7*tempTime+0.6*(1-tempTime),
                                    0.7*tempTime+0.6*(1-tempTime),
                                    0.7*tempTime+0.6*(1-tempTime);
    }
    gaitData._stanceProportion<<1-gaitData._swingProportion(0),
                                1-gaitData._swingProportion(1),
                                1-gaitData._swingProportion(2),
                                1-gaitData._swingProportion(3);
    gaitData._swingTime<<   gaitData._swingProportion(0) * gaitData._periodTime,
                            gaitData._swingProportion(1) * gaitData._periodTime,
                            gaitData._swingProportion(2) * gaitData._periodTime,
                            gaitData._swingProportion(3) * gaitData._periodTime;
    gaitData._stanceTime<<  gaitData._stanceProportion(0) * gaitData._periodTime,
                            gaitData._stanceProportion(1) * gaitData._periodTime,
                            gaitData._stanceProportion(2) * gaitData._periodTime,
                            gaitData._stanceProportion(3) * gaitData._periodTime;
    if(tempTime==1.0)
    {
        tempTime=0.0;
        gaitData._gaitChangeFlag=0;
        gaitData.current_gait=gaitData.next_gait;
    }
}

/**
 * @brief  Gait transition to STAND.
 */
template <typename T>
void GaitScheduler<T>::TransitionToStand()
{
    
}

/**
 * @brief  Gait transition to JUMP.
 */
template <typename T>
void GaitScheduler<T>::TransitionToJump()
{   
    createGait(gaitData.next_gait);
    gaitData._gaitChangeFlag=0;
}

/**
 * @brief  Gait transition to BOUNCE.
 */
template <typename T>
void GaitScheduler<T>::TransitionToBounce()
{   
    createGait(gaitData.next_gait);
    gaitData._gaitChangeFlag=0;
}


/**
 * @brief  Change the gait type.
 * @param  gait_type:Specify the desired gait type. 
 * @return true: change 
 */
template <typename T>
bool GaitScheduler<T>::changeGait(GaitType gait_type)
{   
    
    if(gaitData._exeStanceEndFlag==1)
        gaitData._exeStanceEndFlag=0;

    if(gaitData.current_gait!=gait_type)
    {
        gaitData.next_gait=gait_type;
        gaitData._gaitChangeFlag=1; 
        return true;
    }
    return false;
}

/**
 * @brief  Start the gait generator.(start from STAND to WALK)
 * @note   The core function in this file.
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::gaitGenerate()
{      
    if(gaitData._cycleIndex==1)
    {
        gaitData._cycleIndex=0;
        gaitData._runTimes++;
        if (gaitData._gaitChangeFlag==1)
        {
            //gaitData._gaitInterimFlag=1;
            switch (gaitData.next_gait)
            {
                case GaitType::STAND_UP:
                    TransitionToStand();
                    break;
                case GaitType::WALK:
                    TransitionToWalk();         
                    break;
                case GaitType::TROT:                    
                    TransitionToTrot();                    
                    break;
                case GaitType::CANTER:                    
                    TransitionToCanter();
                    break;
                case GaitType::GALLOP:
                    TransitionToGallop();
                    break;
                case GaitType::JUMP:
                    TransitionToJump();
                    break;
                case GaitType::BOUNCE:
                    TransitionToBounce();
                    break;
            }
        }
    }
        if(gaitData._exeStanceEndFlag!=1 && gaitData._endJumpFlag!=1)
            /*All type's trajectory generate by the PolyTraj(foot_id). */
            for (int foot_id = 0; foot_id < 4; foot_id++)
            {
                PolyTraj(foot_id);
            }

        for (int id = 0; id< 4; id++)
        {
            if(gaitData._phase>=gaitData._phaseOffset(id)&&gaitData._phase-gaitData._dPhase<=gaitData._phaseOffset(id))
            {
                gaitData._trajFootState(id)=1;
            }
            if(gaitData._phaseOffset(id)+gaitData._swingProportion(id)<1.0)
            {
                if (gaitData._phase>=gaitData._phaseOffset(id)+gaitData._swingProportion(id)&&gaitData._phase-gaitData._dPhase<=gaitData._phaseOffset(id)+gaitData._swingProportion(id))
                {
                    gaitData._trajFootState(id)=0;
                }
            }
            else if (gaitData._phaseOffset(id)+gaitData._swingProportion(id)>=1.0)
            {
                if (gaitData._phase>=gaitData._phaseOffset(id)+gaitData._swingProportion(id)-1.0&&gaitData._phase-gaitData._dPhase<=gaitData._phaseOffset(id)+gaitData._swingProportion(id)-1.0)
                {
                    gaitData._trajFootState(id)=0;
                }
            }
        }
        if(gaitData._phase == 1.0)
        {
            gaitData._phase=0.0;
            gaitData._cycleIndex=1;
            if(gaitData.current_gait==GaitType::STAND_UP && gaitData.next_gait==GaitType::STAND_UP)
                gaitData._exeStanceEndFlag=1;
            if ((gaitData.current_gait==GaitType::JUMP && gaitData.next_gait==GaitType::JUMP) ||(gaitData.current_gait==GaitType::BOUNCE && gaitData.next_gait==GaitType::BOUNCE))
                gaitData._endBeforeJumpFlag=1;
        }

        gaitData._phase += gaitData._dPhase;
        std::cout<<gaitData._phase<<std::endl;
        if (gaitData._phase >= 1.0)
        {
            if(gaitData._phase > 1.0)
            gaitData._phase=1.0;
        }
}

/*========================= Trajecory Part of Gait Scheduler ============================*/
/**
 * @brief  Set the trajectory type.
 * @note   Considering that we firstly test the algorithm and see if it works,
 *         only sinusoidal & cycloid can be used.
 * @param  traj_type:Specifies the trajectory type(which can be sinusoidal,cycloid,LPI,polynomial).
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::SetTrajType(int traj_type)
{
    gaitData._trajType = traj_type;
}

/**
 * @brief  Set the update interval for the trajectory point.
 * @param  dPhase:The update interval which should be 0.0 to 0.1 or as small as possible.
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::SetPhaseInterval(T dPhase)
{
    gaitData._dPhase=dPhase;
}

template <typename T>
void GaitScheduler<T>::SetCoMVelocity(T vx, T vy, T vz)
{
    gaitData._v_CoM(0) =vx;
    gaitData._v_CoM(1) =vy;
    gaitData._v_CoM(2) =vz;
}

template <typename T>
void GaitScheduler<T>::SetExeBoundJumpTime(T boundTime)
{
    gaitData.exeBoundJumpTime=boundTime;
}

template <typename T>
void GaitScheduler<T>::SetExeRetractTime(T exeRetractTime)
{
    gaitData.exeRetractTime=exeRetractTime;
}

template <typename T>
void GaitScheduler<T>::SetFallTime(T stfall)
{
    gaitData.exeFallTime=stfall;
}

template <typename T>
void GaitScheduler<T>::SetStopExeRetractTime(T stopExeRetractTime)
{
    gaitData.exeStopRetractTime=stopExeRetractTime;
}

template <typename T>
void GaitScheduler<T>::SetEndWholeJumpTime(T endWholeJumpTime)
{
    gaitData.endWholeJumpTime=endWholeJumpTime;
}

template <typename T>
void GaitScheduler<T>::SetInterPolationPorpotion(T propotion)
{
    gaitData._interPolationPropotion=propotion;
}

template <typename T>
void GaitScheduler<T>::SetSampleFactor(T sampleFactor)
{
    gaitData._sampleFactor=sampleFactor;
}
/**
 * @brief  Set the desired height for the trajectory.
 * @param  traj_height:The desired height which is a floating point value.
 * @retval None.
 */
template <typename T>
void GaitScheduler<T>::SetTrajHeight(T traj_height,int foot_id)
{
    for(int foot_id;foot_id<4;foot_id++)
    {
    gaitData._trajHgt(foot_id) = traj_height;
    }
}

template <typename T>
GaitType GaitScheduler<T>::GetNextGaitType()
{
    return gaitData.next_gait;
}

template <typename T>
GaitType GaitScheduler<T>::GetCurrentGaitType()
{
    return gaitData.current_gait;
}

template <typename T>
int  GaitScheduler<T>::GetGaitChangeFlag()
{
    return gaitData._gaitChangeFlag;
}

/**
 * @brief   Get 
 * @retval  1:Execution finished.
 *          0:Execution not finished or NOT in stance state.
 */
template <typename T>
int  GaitScheduler<T>::GetExeStanceFlag()
{
    return gaitData._exeStanceEndFlag;
}

template <typename T>
int  GaitScheduler<T>::GetEndRetractFlag()
{
    return gaitData._endRetractFlag;
}
/**
 * @brief  Get currunt foot state in the TRAJECTORY.
 * @param  foot_id:The foot that is chosen.
 * @retval 1:The foot state is in the swing.
 *         0:The foot state is in the stance.
 */
template <typename T>
int  GaitScheduler<T>::GetFootState(int foot_id)
{
    return gaitData._trajFootState(foot_id);
}

/**
 * @brief  Get currunt foot position in the TRAJECTORY.
 * @param  foot_id:The foot that is chosen.
 * @retval The currunt foot position in the TRAJECTORY.
 */
template <typename T>
const Vec3<T>& GaitScheduler<T>::GetFootPos(int foot_id)
{
    return gaitData.pFoot_cmd[foot_id];
}

/**
 * @brief  Get currunt foot velocity in the TRAJECTORY.
 * @param  foot_id:The foot that is chosen.
 * @retval The currunt foot velocity in the TRAJECTORY.
 */
template <typename T>
const Vec3<T>& GaitScheduler<T>::GetFootVel(int foot_id)
{
    return gaitData.vFoot_cmd[foot_id];
}

template <typename T>
Vec3<T>  GaitScheduler<T>::GetDebugv_d()
{
    return gaitData._vBody;
}

/**
 * @brief  Get currunt foot acceleration in the TRAJECTORY.
 * @param  foot_id:The foot that is chosen.
 * @retval The currunt foot acceleration in the TRAJECTORY.
 */
template <typename T>
const Vec3<T>& GaitScheduler<T>::GetFootAcc(int foot_id)
{
    return gaitData.aFoot_cmd[foot_id];
}

template <typename T>
const Vec3<T>& GaitScheduler<T>::GetVBody()
{
    return gaitData._vBody;
}

template <typename T>
T GaitScheduler<T>::GetCurrentPhase()
{
    return gaitData._phase;
}

template <typename T>
T GaitScheduler<T>::GetCurrentDPhase()
{
    return gaitData._dPhase;
}

template <typename T>
int GaitScheduler<T>::GetRealFootState(int foot_id)
{
    return gaitData._RealFootState(foot_id);
}

/**
*@brief     Get the phase of remaining time in stance time.    
*/
template <typename T>
T GaitScheduler<T>::GetPhaseInStance(int foot_id)
{
    gaitData._stanceTimeRemaining(foot_id)=gaitData._phaseOffset(foot_id)+1.0-gaitData._phase;

    gaitData._stanceTime(foot_id)=gaitData._stanceProportion(foot_id)*gaitData._periodTime;

    gaitData._phaseInStance(foot_id)=gaitData._stanceTimeRemaining(foot_id)/gaitData._stanceTime(foot_id);
    return gaitData._phaseInStance(foot_id);
}

/**
 * @brief   Get the end flag when the specified foot period is finished.
 * @retval  1:The period has come to an end.
 *          0:The period isn't finished.
 */
template <typename T>
int GaitScheduler<T>::GetPeriodEndFlag()
{
    return gaitData._cycleIndex;
}


template <typename T>
int GaitScheduler<T>::GetEndBeforeJumpFlag()
{
    return gaitData._endBeforeJumpFlag;
}

template <typename T>
int GaitScheduler<T>::GetEndBoundJumpFlag()
{
    return gaitData._endBoundJumpFlag;
}

template <typename T>
int GaitScheduler<T>::GetEndWholeJumpFlag()
{
    return gaitData._endJumpFlag;
}
/**
 * @brief   Get the end flag when the specified foot period is finished.
 * @retval  1:The period has come to an end.
 *          0:The period isn't finished.
 */
template <typename T>
int GaitScheduler<T>::GetSwingEndFlag(int foot_id)
{
    return gaitData._swingEndFlag(foot_id);
}

template <typename T>
T GaitScheduler<T>::GetPeriod()
{
    return gaitData._periodTime;
}

template <typename T>
T GaitScheduler<T>::GetStanceTime(int foot_id)
{
    return gaitData._stanceTime(foot_id);
}

template <typename T>
void  GaitScheduler<T>::JudgeFootState(ContactState outsideInput,int id)
{
    if(outsideInput==swingState)
        if(gaitData._trajFootState(id)==stanceState)
        {
            gaitData._RealFootState(id)=swingState;
            gaitData._DelayExeStanceFlag(id)=1;//Execute trajectory planning.
            gaitData._PreExeStanceFlag(id)=0;
        }
    if(outsideInput==stanceState)
        if(gaitData._trajFootState(id)==swingState)
        {
            gaitData._RealFootState(id)=stanceState;
            gaitData._PreExeStanceFlag(id)=1;//Execute velocity-based stance-phase-planning.
            gaitData._DelayExeStanceFlag(id)=0;
        }
    if(outsideInput==swingState)
        if(gaitData._trajFootState(id)==swingState)
        {
            //Nothing to Do
            gaitData._RealFootState(id)=swingState;
            gaitData._PreExeStanceFlag(id)=0;
            gaitData._DelayExeStanceFlag(id)=0;
        }
    if(outsideInput==stanceState)
        if(gaitData._trajFootState(id)==stanceState)
        {
            //Nothing to Do
            gaitData._RealFootState(id)=stanceState;
            gaitData._PreExeStanceFlag(id)=0;
            gaitData._DelayExeStanceFlag(id)=0;
        }
}

/**
 * @brief  Initialize the trajectory parameters.
 */
template <typename T>
void GaitScheduler<T>::TrajParamSetZero()
{
    Vec3<T> zero= Vec3<T>::Zero();
    for (int id = 0; id < 4; id++)
    {
        gaitData._trajP[id] << 0, 0, 0;
        gaitData._trajV[id] << 0, 0, 0;
        gaitData._trajA[id] << 0, 0, 0;
    }
    gaitData._vBody.setZero();
    gaitData._cycleIndex = 0;
}

/**
 * @brief   Initialize the trajectory parameters.
 * @note    This function is called in "GaitScheduler.initailize()".
 * @param   traj_height:The desired height which is a floating point value.
 * @param   traj_type:Specifies the trajectory type which can be sin,cycloid,polynomial or LPI.
 * @param   trajSche_type:Specifies the type of the trajectory scheduler 
 *                        which can be joint space or cartesian space. 
 * @retval  None.
 */
template <typename T>
void GaitScheduler<T>::Traj_Param_Init( T traj_height, int traj_type, int trajSche_type)
{
    for (int foot_id = 0; foot_id < 4; foot_id++)
    {
        gaitData._trajHgt(foot_id) = traj_height;
    }

    gaitData._trajType = traj_type;
    gaitData._trajScheType = trajSche_type;
}

template <typename T>
void GaitScheduler<T>::CanterTraj(int foot_id)
{
    gaitData._pCanterStretch[foot_id]=
    gaitData._pStart[foot_id]*(1-gaitData._sampleFactor)+gaitData._pFinal[foot_id]*gaitData._sampleFactor+(Vec3<T>(0,0,-0.1));

    if(gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)>1.0)
    {
        if (gaitData._phase+1.0>=gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)&&gaitData._phase<gaitData._phaseOffset(foot_id))
        {
            //interpolation
            if (gaitData._phase<(gaitData._phaseOffset(foot_id)-gaitData._stanceProportion(foot_id)+gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion))
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pFinal[foot_id]*(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion-((gaitData._phase+1.0)-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id)))/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)
                +gaitData._pCanterStretch[foot_id]*((gaitData._phase+1.0)-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id))/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion);
            }
            else
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pCanterStretch[foot_id]*(gaitData._phaseOffset(foot_id)-gaitData._phase)/((1-gaitData._interPolationPropotion)*gaitData._stanceProportion(foot_id))
                +gaitData._pStart[foot_id]*(gaitData._phase-(gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)-1.0+gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion))/((1-gaitData._interPolationPropotion)*gaitData._stanceProportion(foot_id));
            } 
        }
        else
            PolyTraj(foot_id);
    }
    else if(gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)<1.0)
    {
        if (gaitData._phase>gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id))
        {
            if (gaitData._phase<gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)+gaitData._interPolationPropotion*gaitData._stanceProportion(foot_id))
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pFinal[foot_id]*(gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)+gaitData._interPolationPropotion*gaitData._stanceProportion(foot_id)-gaitData._phase)/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)
                +gaitData._pCanterStretch[foot_id]*(gaitData._phase-gaitData._swingProportion(foot_id))/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion);
            }
            else
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pCanterStretch[foot_id]*(1.0-gaitData._phase)/(gaitData._stanceProportion(foot_id)*(1-gaitData._interPolationPropotion))
                +gaitData._pStart[foot_id]*(gaitData._phase-gaitData._swingProportion(foot_id)-gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)/(gaitData._stanceProportion(foot_id)*(1.0-gaitData._interPolationPropotion));                
            }
        }
        else
            PolyTraj(foot_id);
    }
    else if(gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)==1.0)
    {
        if (gaitData._phase<gaitData._phaseOffset(foot_id))
        {
            if (gaitData._phase<gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pFinal[foot_id]*(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion-gaitData._phase)/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)
                +gaitData._pCanterStretch[foot_id]*gaitData._phase/(gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion);
            }
            else
            {
                gaitData.pFoot_cmd[foot_id]=
                gaitData._pCanterStretch[foot_id]*(gaitData._stanceProportion(foot_id)-gaitData._phase)/(gaitData._stanceProportion(foot_id)*(1.0-gaitData._interPolationPropotion))
                +gaitData._pStart[foot_id]*(gaitData._phase-gaitData._stanceProportion(foot_id)*gaitData._interPolationPropotion)/(gaitData._stanceProportion(foot_id)*(1.0-gaitData._interPolationPropotion));
            }
        }
        else
            PolyTraj(foot_id);
    }
}
/**
 * @brief   Create sinusoidal trajectory.
 * @note    This function is called in "GaitScheduler.Traj_Generate()".
 * @param   foot_id:The foot that is chosen.
 */
template <typename T>
void GaitScheduler<T>::StartTraj(int foot_id)
{
    if(gaitData._phase >= gaitData._phaseOffset(foot_id) && gaitData._phase < (gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)))
    {
        gaitData._pdelta  =  gaitData._pFinal[foot_id]  -  gaitData._pInit[foot_id];

        gaitData._trajP[foot_id](0) = gaitData._pInit[foot_id](0) + gaitData._pdelta(0) * ((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id) 
                                    - (sin((2.0 * PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime) / gaitData._swingTime(foot_id))) / (2.0 * PI));
        
        gaitData._trajP[foot_id](1) = gaitData._pInit[foot_id](1) + gaitData._pdelta(1) * ((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id) 
                                    - (sin((2.0 * PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime) / gaitData._swingTime(foot_id))) / (2.0 * PI));
        
        if (gaitData._phase < (gaitData._phaseOffset(foot_id)+ 0.5 * gaitData._swingProportion(foot_id)))
        {
            gaitData._trajP[foot_id](2) = gaitData._trajHgt(foot_id) * sin(PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id)) 
                                        + gaitData._pInit[foot_id](2);
        }
        else 
        {
            gaitData._trajP[foot_id](2) = (gaitData._trajHgt(foot_id) + gaitData._pInit[foot_id](2) - gaitData._pFinal[foot_id](2)) 
                                        * sin(PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id)) + gaitData._pFinal[foot_id](2);
        }
    }
    else if(gaitData._phase < gaitData._phaseOffset(foot_id))
    {
        gaitData._trajP[foot_id]=gaitData._pInit[foot_id]*(gaitData._phaseOffset(foot_id)-gaitData._phase)/gaitData._phaseOffset(foot_id)
                                +gaitData._pStart[foot_id]*gaitData._phase/gaitData._phaseOffset(foot_id);
    }
    else if (gaitData._phase >= gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id))
    {
        gaitData._trajP[foot_id]=gaitData._pFinal[foot_id]*(gaitData._stanceProportion(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id)))/gaitData._stanceProportion(foot_id)
                                +gaitData._pStart[foot_id]*(gaitData._phase-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id))/gaitData._stanceProportion(foot_id);
    }

    gaitData.pFoot_cmd[foot_id] = gaitData._trajP[foot_id];
}

/**
 * @brief   Create sinusoidal trajectory.
 * @note    This function is called in "GaitScheduler.Traj_Generate()".
 * @param   foot_id:The foot that is chosen.
 */
template <typename T>
void GaitScheduler<T>::SinTraj(int foot_id)
{
    gaitData._pdelta  =  gaitData._pFinal[foot_id]  -  gaitData._pStart[foot_id];

    if(gaitData._phase >= gaitData._phaseOffset(foot_id) && gaitData._phase < (gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)))
    {
        gaitData._trajP[foot_id](0) = gaitData._pStart[foot_id](0) + gaitData._pdelta(0) * ((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id) 
                                    - (sin((2.0 * PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime) / gaitData._swingTime(foot_id))) / (2.0 * PI));
        
        gaitData._trajP[foot_id](1) = gaitData._pStart[foot_id](1) + gaitData._pdelta(1) * ((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id) 
                                    - (sin((2.0 * PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime) / gaitData._swingTime(foot_id))) / (2.0 * PI));
        
        
        if (gaitData._phase < (gaitData._phaseOffset(foot_id)+ 0.5 * gaitData._swingProportion(foot_id)))
        {
            gaitData._trajP[foot_id](2) = gaitData._trajHgt(foot_id) * sin(PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id)) 
                                        + gaitData._pStart[foot_id](2);
        }
        else 
        {
            gaitData._trajP[foot_id](2) = (gaitData._trajHgt(foot_id) + gaitData._pStart[foot_id](2) - gaitData._pFinal[foot_id](2)) 
                                        * sin(PI * (gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime / gaitData._swingTime(foot_id)) + gaitData._pFinal[foot_id](2);
        }
    }
            // gaitData._trajP[foot_id]/=100;
    else 
    {
        if (gaitData.current_gait==GaitType::STAND_UP && gaitData.next_gait == GaitType::STAND_UP)
        {
            gaitData._trajP[foot_id]=gaitData._pStart[foot_id]*(1-gaitData._phase)
                                    +gaitData._pFinal[foot_id]*gaitData._phase;
        }
        else
        {
            if(gaitData._phase >= gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id))
            {
                gaitData._trajP[foot_id]=gaitData._pFinal[foot_id]*(gaitData._stanceProportion(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id)))/gaitData._stanceProportion(foot_id)
                                        +gaitData._pStart[foot_id]*(gaitData._phase-gaitData._phaseOffset(foot_id)-gaitData._swingProportion(foot_id))/gaitData._stanceProportion(foot_id);
            }
            else if(gaitData._phase < gaitData._phaseOffset(foot_id))
            {
                gaitData._trajP[foot_id]=gaitData._pFinal[foot_id]*(gaitData._phaseOffset(foot_id)-gaitData._phase)/gaitData._stanceProportion(foot_id)
                                        +gaitData._pStart[foot_id]*(gaitData._stanceProportion(foot_id)+gaitData._phase-gaitData._phaseOffset(foot_id))/gaitData._stanceProportion(foot_id);
            }
        }
    }
    gaitData.pFoot_cmd[foot_id] = gaitData._trajP[foot_id];
}

/**
 * @brief   Create polynomail trajectory.
 * @note    This function is called in "GaitScheduler.Traj_Generate()".
 * @param   foot_id:The foot that is chosen.
 */
template <typename T>
void GaitScheduler<T>::PolyTraj(int foot_id)
{
    gaitData._pdelta  =  gaitData._pFinal[foot_id]  -  gaitData._pStart[foot_id];

    if(gaitData._DelayExeStanceFlag(foot_id)==1)//If DelayFlag=1,it will delay in the stance-execution.
    {
        gaitData._pFinal[foot_id]=gaitData._pFinal[foot_id]+(Vec3<T>(0,0,-0.02));
        gaitData._trajP[foot_id]=gaitData._pFinal[foot_id];
        gaitData._trajV[foot_id]=(Vec3<T>(0,0,-0.02))/(gaitData._dPhase*gaitData._periodTime);
        gaitData._trajA[foot_id]<<0,0,0;
    }
    else if(gaitData._PreExeStanceFlag(foot_id)!=1)//if PreFlag=1, it will leave the Polytraj and  execute stance ahead of time.
    {    
        if ((gaitData.current_gait==GaitType::STAND_UP && gaitData.next_gait == GaitType::STAND_UP)||(gaitData.current_gait==GaitType::JUMP && gaitData.next_gait == GaitType::JUMP)||(gaitData.current_gait==GaitType::BOUNCE && gaitData.next_gait == GaitType::BOUNCE))
        {
            if(gaitData._endBeforeJumpFlag==1)//Footposition has come to before-jump-position.
            {
                // if (gaitData._phase==1.0)
                //     gaitData._endJumpFlag=1;

                if (gaitData._phase<=gaitData.exeBoundJumpTime)
                {
                    gaitData._trajP[foot_id]=gaitData._pStart[foot_id]*(gaitData.exeBoundJumpTime-gaitData._phase)/gaitData.exeBoundJumpTime
                                    +gaitData._pFinal[foot_id]*gaitData._phase/gaitData.exeBoundJumpTime;
                }
                if (gaitData._phase>gaitData.exeBoundJumpTime)
                {
                    gaitData._endBoundJumpFlag=1;
                }
                if (gaitData._endBoundJumpFlag==1 && gaitData._phase>=gaitData.exeRetractTime && gaitData._phase<gaitData.exeStopRetractTime)
                {
                    gaitData._trajP[foot_id]=gaitData._pStart[foot_id]
                                            *(gaitData.exeStopRetractTime-gaitData._phase)/(gaitData.exeStopRetractTime-gaitData.exeRetractTime)
                                            +gaitData._pFinal[foot_id]*(gaitData._phase-gaitData.exeRetractTime)/(gaitData.exeStopRetractTime-gaitData.exeRetractTime);
                }
                if (gaitData._phase >= gaitData.exeStopRetractTime && gaitData._phase < gaitData.endWholeJumpTime)
                {
                    gaitData._endRetractFlag=1;
                    if (gaitData._phase>gaitData.exeFallTime)
                    {
                        gaitData._trajP[foot_id]=gaitData._pStart[foot_id]*(gaitData.endWholeJumpTime-gaitData._phase)/(gaitData.endWholeJumpTime-gaitData.exeFallTime)
                                    +gaitData._pFinal[foot_id]*(gaitData._phase-gaitData.exeFallTime)/(gaitData.endWholeJumpTime-gaitData.exeFallTime);
                    }
                }
                if(gaitData._phase > gaitData.endWholeJumpTime)
                {
                    gaitData._endJumpFlag=1;
                }
            }
            else
            {
                gaitData._trajP[foot_id]=gaitData._pStart[foot_id]*(1-gaitData._phase)
                                    +gaitData._pFinal[foot_id]*gaitData._phase;
            }
        }
        else//Currunt gait isn't WALK.
        {
            if (gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)<=1.0)
            {
                if(gaitData._phase >= gaitData._phaseOffset(foot_id) && gaitData._phase < (gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id)))
                {
                    double t5=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0);
                    double t4=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0);
                    double t3=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0);
                    double t2=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0);
                    double t1=(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime;
                    double x5=pow(gaitData._swingTime(foot_id),5.0);
                    double x4=pow(gaitData._swingTime(foot_id),4.0);
                    double x3=pow(gaitData._swingTime(foot_id),3.0);
                    gaitData._trajP[foot_id]= gaitData._pStart[foot_id]
                                                +6.0*gaitData._pdelta*t5/x5
                                                -15.0*gaitData._pdelta*t4/x4
                                                +10.0*gaitData._pdelta*t3/x3;   
                    gaitData._trajV[foot_id]=5*6.0*gaitData._pdelta*t4/x5
                                                -4*15.0*gaitData._pdelta*t3/x4
                                                +3*10.0*gaitData._pdelta*t2/x3;
                    gaitData._trajA[foot_id]=4*5*6.0*gaitData._pdelta*t3/x5
                                                -3*4*15.0*gaitData._pdelta*t2/x4
                                                +2*3*10.0*gaitData._pdelta*t1/x3;                                                           
                    if (gaitData._phase < (gaitData._phaseOffset(foot_id)+ 0.5 * gaitData._swingProportion(foot_id)))
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pStart[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*t5/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*t4/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*t3/x3;
                        gaitData._trajV[foot_id](2)=5*192*gaitData._trajHgt(foot_id)*t4/x5
                                                    -4*240*gaitData._trajHgt(foot_id)*t3/x4
                                                    +3*80.0*gaitData._trajHgt(foot_id)*t2/x3;
                        gaitData._trajA[foot_id](2)=4*5*192*gaitData._trajHgt(foot_id)*t3/x5
                                                    -3*4*240*gaitData._trajHgt(foot_id)*t2/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*t1/x3;                            
                    }
                    else 
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pFinal[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0)/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x3;
                        gaitData._trajV[foot_id](2)=-5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x5
                                                    +4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x4
                                                    -3*80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x3;
                        gaitData._trajA[foot_id](2)=+4*5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x5
                                                    -3*4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime)/x3;
                    }
                }
            }
            else if (gaitData._swingProportion(foot_id)+gaitData._phaseOffset(foot_id)>1.0)
            {
                if(gaitData._phase >= gaitData._phaseOffset(foot_id))
                {
                    double t5=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0);
                    double t4=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0);
                    double t3=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0);
                    double t2=pow((gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0);
                    double t1=(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime;
                    double x5=pow(gaitData._swingTime(foot_id),5.0);
                    double x4=pow(gaitData._swingTime(foot_id),4.0);
                    double x3=pow(gaitData._swingTime(foot_id),3.0);
                    gaitData._trajP[foot_id]= gaitData._pStart[foot_id]
                                                +6.0*gaitData._pdelta*t5/x5
                                                -15.0*gaitData._pdelta*t4/x4
                                                +10.0*gaitData._pdelta*t3/x3;   
                    gaitData._trajV[foot_id]=5*6.0*gaitData._pdelta*t4/x5
                                                -4*15.0*gaitData._pdelta*t3/x4
                                                +3*10.0*gaitData._pdelta*t2/x3;
                    gaitData._trajA[foot_id]=4*5*6.0*gaitData._pdelta*t3/x5
                                                -3*4*15.0*gaitData._pdelta*t2/x4
                                                +2*3*10.0*gaitData._pdelta*t1/x3;                                                           
                    if (gaitData._phase < (gaitData._phaseOffset(foot_id)+ 0.5 * gaitData._swingProportion(foot_id)))
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pStart[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*t5/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*t4/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*t3/x3;
                        gaitData._trajV[foot_id](2)=5*192*gaitData._trajHgt(foot_id)*t4/x5
                                                    -4*240*gaitData._trajHgt(foot_id)*t3/x4
                                                    +3*80.0*gaitData._trajHgt(foot_id)*t2/x3;
                        gaitData._trajA[foot_id](2)=4*5*192*gaitData._trajHgt(foot_id)*t3/x5
                                                    -3*4*240*gaitData._trajHgt(foot_id)*t2/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*t1/x3;                            
                    }
                    else 
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pFinal[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0)/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x3;
                        gaitData._trajV[foot_id](2)=-5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x5
                                                    +4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x4
                                                    -3*80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x3;
                        gaitData._trajA[foot_id](2)=+4*5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x5
                                                    -3*4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*(gaitData._swingTime(foot_id)-(gaitData._phase-gaitData._phaseOffset(foot_id))*gaitData._periodTime)/x3;
                    }
                }
                else if (gaitData._phase<gaitData._phaseOffset(foot_id) && gaitData._phase+1.0<gaitData._phaseOffset(foot_id)+gaitData._swingProportion(foot_id))
                {
                    double t5=pow((gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0);
                    double t4=pow((gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0);
                    double t3=pow((gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0);
                    double t2=pow((gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0);
                    double t1=(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime;
                    double x5=pow(gaitData._swingTime(foot_id),5.0);
                    double x4=pow(gaitData._swingTime(foot_id),4.0);
                    double x3=pow(gaitData._swingTime(foot_id),3.0);
                    gaitData._trajP[foot_id]= gaitData._pStart[foot_id]
                                                +6.0*gaitData._pdelta*t5/x5
                                                -15.0*gaitData._pdelta*t4/x4
                                                +10.0*gaitData._pdelta*t3/x3;   
                    gaitData._trajV[foot_id]=5*6.0*gaitData._pdelta*t4/x5
                                                -4*15.0*gaitData._pdelta*t3/x4
                                                +3*10.0*gaitData._pdelta*t2/x3;
                    gaitData._trajA[foot_id]=4*5*6.0*gaitData._pdelta*t3/x5
                                                -3*4*15.0*gaitData._pdelta*t2/x4
                                                +2*3*10.0*gaitData._pdelta*t1/x3;            
                    if (gaitData._phase+1.0 < (gaitData._phaseOffset(foot_id)+ 0.5 * gaitData._swingProportion(foot_id)))
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pStart[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*t5/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*t4/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*t3/x3;
                        gaitData._trajV[foot_id](2)=5*192*gaitData._trajHgt(foot_id)*t4/x5
                                                    -4*240*gaitData._trajHgt(foot_id)*t3/x4
                                                    +3*80.0*gaitData._trajHgt(foot_id)*t2/x3;
                        gaitData._trajA[foot_id](2)=4*5*192*gaitData._trajHgt(foot_id)*t3/x5
                                                    -3*4*240*gaitData._trajHgt(foot_id)*t2/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*t1/x3;                            
                    }
                    else 
                    {
                        gaitData._trajP[foot_id](2)= gaitData._pFinal[foot_id](2)
                                                    +192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,5.0)/x5
                                                    -240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x4
                                                    +80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x3;
                        gaitData._trajV[foot_id](2)=-5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,4.0)/x5
                                                    +4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x4
                                                    -3*80.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x3;
                        gaitData._trajA[foot_id](2)=+4*5*192.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,3.0)/x5
                                                    -3*4*240.0*gaitData._trajHgt(foot_id)*pow(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime,2.0)/x4
                                                    +2*3*80.0*gaitData._trajHgt(foot_id)*(gaitData._swingTime(foot_id)-(gaitData._phase+1.0-gaitData._phaseOffset(foot_id))*gaitData._periodTime)/x3;
                    }
                }
            }     
        }
    }   
    gaitData.pFoot_cmd[foot_id] = gaitData._trajP[foot_id];
    gaitData.vFoot_cmd[foot_id] = gaitData._trajV[foot_id];
    gaitData.aFoot_cmd[foot_id] = gaitData._trajA[foot_id];
}


/**
 * @brief   Show trajectory information.
 */
template <typename T>
void GaitScheduler<T>::ShowInfo()
{
    gaitData._trajInfo << gaitData._phase,
    gaitData.pFoot_cmd[LegsName::RF](0),
    gaitData.pFoot_cmd[LegsName::RF](1),
    gaitData.pFoot_cmd[LegsName::RF](2),
    gaitData._phase,
    gaitData.pFoot_cmd[LegsName::LF](0),
    gaitData.pFoot_cmd[LegsName::LF](1),
    gaitData.pFoot_cmd[LegsName::LF](2),
    gaitData._phase,
    gaitData.pFoot_cmd[LegsName::LB](0),
    gaitData.pFoot_cmd[LegsName::LB](1),
    gaitData.pFoot_cmd[LegsName::LB](2),
    gaitData._phase,
    gaitData.pFoot_cmd[LegsName::RB](0),
    gaitData.pFoot_cmd[LegsName::RB](1),
    gaitData.pFoot_cmd[LegsName::RB](2);
    std::cout   << "Position of Foot" << std::endl
                << "Sequence:RF->LF->LB->RB" << std::endl;
    std::cout << "phase         x        y       z" << std::endl;

    std::cout << gaitData._trajInfo << std::endl;
}

template <typename T>
void GaitScheduler<T>::ClearJumpFlag()
{
    gaitData._endBeforeJumpFlag=0;
    gaitData._endBoundJumpFlag=0;
    gaitData._endRetractFlag=0;
    gaitData._endJumpFlag=0;
}

template <typename T>
GaitScheduler<T>::GaitScheduler(/* args */)
{
}
template <typename T>
GaitScheduler<T>::~GaitScheduler()
{
}

template class GaitScheduler<double>;
template class GaitScheduler<float>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/
